/*
 * e-motion control http://code.google.com/p/e-motioncontrol/
 * 
 * Copyright (c) 2008-2009,
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms,
 * with or without modification, are permitted provided
 * that the following conditions are met:
 *    - Redistributions of source code must retain the
 *      above copyright notice, this list of conditions
 *      and the following disclaimer.
 *    - Redistributions in binary form must reproduce the
 *      above copyright notice, this list of conditions and
 *	the following disclaimer in the documentation and/or
 *	other materials provided with the distribution.
 *    - Neither the name of the e-motioncontrol nor the names
 *      of its contributors may be used to endorse or promote
 *	products derived from this software without specific
 *	prior written permission.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICTLIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
 * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <stdlib.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <math.h>

#include "include/wiimote.h"
#include "include/errors.h"
#include "include/trajectory.h"

/** Trajectory computing functions file.
 * @file
 */

int dir_to_int(char *dir)
{
  if (strcmp(dir, "E") == 0)
    return E;
  if (strcmp(dir, "NE") == 0)
    return NE;
  if (strcmp(dir, "N") == 0)
    return N;
  if (strcmp(dir, "NW") == 0)
    return NW;
  if (strcmp(dir, "W") == 0)
    return W;
  if (strcmp(dir, "SW") == 0)
    return SW;
  if (strcmp(dir, "S") == 0)
    return S;
  if (strcmp(dir, "SE") == 0)
    return SE;
  else return -1;
}

char *int_to_dir(int dir)
{
  if (dir == E)
    return "E";
  if (dir == NE)
    return "NE";
  if (dir == N)
    return "N";
  if (dir == NW)
    return "NW";
  if (dir == W)
    return "W";
  if (dir == SW)
    return "SW";
  if (dir == S)
    return "S";
  if (dir == SE)
    return "SE";
  else return "";
}

float compute_angle(float dx, float dy)
{
  float x1 = 1;
  float y1 = 0;	
  float x2 = dx;
  float y2 = dy;
  int sign = 1;
  float scal = (x1 * x2) + (y1 * y2);
  float norm = sqrt((x1 * x1) + (y1 * y1)) * sqrt((x2 * x2) + (y2 * y2));
  float alpha;
  
  if (y2 - y1 < 0)
    sign = -1;
  alpha = sign * acos(scal / norm);
  return alpha;
}

int get_orientation(struct wiimote_t* wm)
{
	int i;	
	float x1 = 0, y1 = 0, x2 = 0, y2 = 0;
	float dx, dy, alpha;
	
	if ((wm->ir.ax != wm->lstate.ir_ax) || (wm->ir.ay != wm->lstate.ir_ay)) {
		for (i = 0; i <= MAX_STATES / 2; i++) {
	  		x1 = x1 + wm->pstates[i].ir_ax;
			y1 = y1 + wm->pstates[i].ir_ay;
			x2 = x2 + wm->pstates[MAX_STATES - i - 1].ir_ax;
			y2 = y2 + wm->pstates[MAX_STATES - i - 1].ir_ay;
	  	}
	}
	
	dx = x2 - x1;
  	dy = - y2 + y1;

	alpha = compute_angle(dx, dy);

  if ((abs(dx) >= POS_VAR) || (abs(dy) >= POS_VAR))	
    wm->ir.dir = alpha;
  else
    return (-1);
  
  if (OR_E(wm->ir.dir))
    return(E);
  if (OR_S(wm->ir.dir))
    return(S);
  if (OR_N(wm->ir.dir))
    return(N);
  if (OR_NE(wm->ir.dir))
    return(NE);
  if (OR_NW(wm->ir.dir))
    return(NW);
  if (OR_SW(wm->ir.dir))
    return(SW);
  if (OR_SE(wm->ir.dir))
    return(SE);
  if (OR_W(wm->ir.dir))
    return(W);
  return(-1);
}
